#include <serial/serial.h>
#include<iostream>

#define GET_LOW_BYTE(A) (uint8_t)((A))
#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
#define BYTE_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))

#define LOBOT_SERVO_FRAME_HEADER         85
#define LOBOT_SERVO_MOVE_TIME_WRITE      1
#define LOBOT_SERVO_MOVE_TIME_WAIT_WRITE 7
#define LOBOT_SERVO_MOVE_START           11
#define LOBOT_SERVO_MOVE_STOP            12
#define LOBOT_SERVO_POS_READ             28


class ArmCom{
public:
    ArmCom(){};
    ~ArmCom(){};
    int tandemSpeed[10]={1,1,4,4,4,1,1,1,1,1};
    int ArmComOpen(std::string armPort){
        try{
            (this->ser).setPort(armPort);  //设置端口号
            (this->ser).setBaudrate(115200);  //设置波特率
            serial::Timeout tout = serial::Timeout::simpleTimeout(1000);//设置延时等待 ms
            (this->ser).setTimeout(tout); 
            (this->ser).open(); //打开串
        }
        catch (serial::IOException& e) {
            std::cout<<"aemPort is not open"<<std::endl;
            return 0; 
        } 
        if((this->ser).isOpen()){
            std::cout<<"aemPort is open"<<std::endl;
            return 1;
        } 
        else {
            std::cout<<"aemPort is not open"<<std::endl;
            return 0; 
        } 
    }
    void tandemMove(uint8_t id, int16_t position, uint16_t time){
        unsigned char buf[10];
        if(position < 0)
            position = 0;
        if(position > 1000)
            position = 1000;
        buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
        buf[2] = id;
        buf[3] = 7;
        buf[4] = LOBOT_SERVO_MOVE_TIME_WRITE;
        buf[5] = GET_LOW_BYTE(position);
        buf[6] = GET_HIGH_BYTE(position);
        buf[7] = GET_LOW_BYTE(time);
        buf[8] = GET_HIGH_BYTE(time);
        buf[9] = LobotCheckSum(buf);
        this->ser.write(buf,10); 
    }
    void gotoStart(uint8_t id){
        tandemStartSet(id,tandemPosition(id)*this->tandemSpeed[id]);
        tandemMove(id);
    }
    void gotoTerminus(uint8_t id){
        tandemTerminusSet(id,1000*this->tandemSpeed[id]-tandemPosition(id)*this->tandemSpeed[id]);
        tandemMove(id);
    }
    int tandemPosition(uint8_t id){
        int pos=LobotSerialServoReadPosition(id);
        while(pos==-1){
            pos=LobotSerialServoReadPosition(id);
        }
        return pos;
    }
    void tandemStop(uint8_t id){
        unsigned char buf[6];
        buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
        buf[2] = id;
        buf[3] = 3;
        buf[4] = LOBOT_SERVO_MOVE_STOP;
        buf[5] = LobotCheckSum(buf);
        this->ser.write(buf, 6);
    }
private:
    serial::Serial ser;
    unsigned char LobotCheckSum(unsigned char buf[]){
        unsigned char i;
        uint16_t temp = 0;
        for (i = 2; i < buf[3] + 2; i++) {
            temp += buf[i];
        }
        temp = ~temp;
        i = (unsigned char)temp;
        return i;
    }
    void tandemStartSet(uint8_t id,uint16_t time){
        unsigned char buf[10];
        uint8_t Mode;
        buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
        buf[2] = id;
        buf[3] = 7;
        buf[4] = LOBOT_SERVO_MOVE_TIME_WAIT_WRITE;
        buf[5] = GET_LOW_BYTE(0);
        buf[6] = GET_HIGH_BYTE(0);
        buf[7] = GET_LOW_BYTE(time);
        buf[8] = GET_HIGH_BYTE(time);
        buf[9] = LobotCheckSum(buf);
        this->ser.write(buf, 10);
    }
    void tandemTerminusSet(uint8_t id,uint16_t time){
        unsigned char buf[10];
        uint8_t Mode;
        buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
        buf[2] = id;
        buf[3] = 7;
        buf[4] = LOBOT_SERVO_MOVE_TIME_WAIT_WRITE;
        buf[5] = GET_LOW_BYTE(1000);
        buf[6] = GET_HIGH_BYTE(1000);
        buf[7] = GET_LOW_BYTE(time);
        buf[8] = GET_HIGH_BYTE(time);
        buf[9] = LobotCheckSum(buf);
        this->ser.write(buf, 10);
    }
    void tandemMove(uint8_t id){
        unsigned char buf[6];
        buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
        buf[2] = id;
        buf[3] = 3;
        buf[4] = LOBOT_SERVO_MOVE_START;
        buf[5] = LobotCheckSum(buf);
        this->ser.write(buf, 6);
    }
    int LobotSerialServoReceiveHandle(unsigned char *ret){
        bool frameStarted = false;
        bool receiveFinished = false;
        unsigned char frameCount = 0;
        unsigned char dataCount = 0;
        unsigned char dataLength = 2;
        unsigned char rxBuf;
        unsigned char recvBuf[32];
        unsigned char i;
        while (this->ser.available()) {
            this->ser.read(&rxBuf,1);
            if (!frameStarted) {
            if (rxBuf == LOBOT_SERVO_FRAME_HEADER) {
                frameCount++;
                if (frameCount == 2) {
                frameCount = 0;
                frameStarted = true;
                dataCount = 1;
                }
            }
            else {
                frameStarted = false;
                dataCount = 0;
                frameCount = 0;
            }
            }
            if (frameStarted) {
                recvBuf[dataCount] = (uint8_t)rxBuf;
                if (dataCount == 3) {
                    dataLength = recvBuf[dataCount];
                    if (dataLength < 3 || dataCount > 7) {
                    dataLength = 2;
                    frameStarted = false;
                    }
                }
                dataCount++;
                if (dataCount == dataLength + 3) {
                    if (LobotCheckSum(recvBuf) == recvBuf[dataCount - 1]) {
                    frameStarted = false;
                    memcpy(ret, recvBuf + 4, dataLength);
                    return 1;
                    }
                    return -1;
                }
            }
        }
    }
    int LobotSerialServoReadPosition(uint8_t id){
        int count = 10000;
        int ret;
        unsigned char buf[6];
        buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
        buf[2] = id;
        buf[3] = 3;
        buf[4] = LOBOT_SERVO_POS_READ;
        buf[5] = LobotCheckSum(buf);
        while (this->ser.available())
            this->ser.read();
        this->ser.write(buf, 6);
        while (!this->ser.available()) {
            count -= 1;
            if (count < 0)
            return -1;
        }
        if (LobotSerialServoReceiveHandle(buf) > 0)
            ret = BYTE_TO_HW(buf[2], buf[1]);
        else
            ret = -1;
        return ret;
    }
};